/** Completed and tested - working **/


#include "slep.h"
#include "hr_time.h"
#include "epph.h"

FUNVAL mcLeastRm1(double* A, double* y, double z, OPTS opt);
FUNVAL mcLeastRm2(double* A, double* y, double z, OPTS opt);
FUNVAL mcLeastRm3(double* A, double* y, double z, OPTS opt);


FUNVAL mcLeastR(double* A, double* y, double z, OPTS opt){
	if(opt.mFlag== 0 && opt.lFlag == 0 )
		return mcLeastRm1(A,y,z,opt);
	else if(opt.mFlag == 1 && opt.lFlag == 0 && DBLCOMPARE(opt.q,2.0) )
		return mcLeastRm2(A,y,z,opt);
	else if(opt.mFlag == 1 && opt.lFlag == 1 && DBLCOMPARE(opt.q,2.0) )
		return mcLeastRm3(A,y,z,opt);
	else
		errorOut("Please set the values of opt.mFlag and opt.lFlag to correct values before executing mcLeastR.");
}


FUNVAL mcLeastRm1(double* A, double* y,double z,OPTS opt){
	int m, n, iterStep, k, i, bFlag;
	double q, lambda, q_bar, lambda_max, L, alphap, alpha, beta, r_sum, l_sum, fVal, fValp;
	double *ATy, *x, *Ax, *xp, *Axp, *xxp, *s, *As, *ATAs, *g, *v, *Av, *Axy, *ValueL, *funVal;
	FUNVAL ret;

	m = opt.m;
	n = opt.n;
	k = opt.mc_k;

	ATy = (double *) malloc( sizeof(double) * n*k);
	x = (double *) malloc( sizeof(double) * n*k);
	Ax = (double *) malloc( sizeof(double) * m*k);
	xp = (double *) malloc( sizeof(double) * n*k);
	Axp = (double *) malloc( sizeof(double) * m*k);
	xxp = (double *) malloc( sizeof(double) * n*k);
	s = (double *) malloc( sizeof(double) * n*k);
	As = (double *) malloc( sizeof(double) * m*k);
	ATAs = (double *) malloc( sizeof(double) * n*k);
	g = (double *) malloc( sizeof(double) * n*k);
	v = (double *) malloc( sizeof(double) * n*k);
	Av = (double *) malloc( sizeof(double) * m*k);
	ValueL = (double *) malloc( sizeof(double) * opt.maxIter);
	Axy = (double *) malloc( sizeof(double) * m*k);
	funVal = (double *) malloc( sizeof(double) * opt.maxIter);

	if(z<= 0)
		errorOut("z should be positive.");

	if(!hasGot(&opt,"q"))
		opt.q = q = 2.0;
	else if( (q = opt.q) < 0.99999)
		errorOut("'q' should be larger than 1.");

	initNormalization(&opt,A);

	// calculate ATy
	normalizedMM('T',m,n,k,A,y,ATy,&opt);

	// Process regularization parameters
	if( opt.rFlag == 0 )
		lambda = z;
	else{
		if(z<0.0 || z>1.0 )
			errorOut("Z should lie in range [0,1] in mcLeastR.");
		if(q==1)
			q_bar= DBL_MAX;
		else if (q>1.0e6)
			q_bar = 1.0;
		else
			q_bar = q/(q-1);
		lambda_max = 0.0;
		for(i=0; i < n ; i++)
			lambda_max = maxof2( lambda_max , dnrm(k,&ATy[i],n,q_bar));
		lambda = z* lambda_max;
	}

	if( opt.init == 2)
		memset(x,0,n*k*sizeof(double));
	else if(hasGot(&opt,"x0"))
		dcopy(n*k,opt.x0,1,x,1);
	else
		dcopy(n*k,ATy,1,x,1);

	// compute Ax
	normalizedMM('N',m,n,k,A,x,Ax,&opt);

	if(opt.init == 0){
		// TO-DO have to do the initfactor module here
	}

	bFlag = 0;

	if (opt.mFlag==0 && opt.lFlag==0){
		L=1.0;
		//xp=x; Axp=Ax; xxp=zeros(n,k);
		dcopy(n*k,x,1,xp,1);
		dcopy(m*k,Ax,1,Axp,1);
		memset(xxp,0,n*k*sizeof(double));

		alphap=0; alpha=1;

		for(iterStep = 0; iterStep < opt.maxIter ; iterStep ++ ){
			beta=(alphap-1)/alpha;

			//s=x + beta* xxp;
			dcopy(n*k,x,1,s,1);
			daxpy(n*k,beta,xxp,1,s,1);

			//As=Ax + beta* (Ax-Axp);
			dcopy(m*k,Ax,1,As,1);
			dscal(m*k,1.0 + beta,As,1);
			daxpy(m*k,-beta,Axp,1,As,1);

			// compute ATAs
			normalizedMM('T',m,n,k,A,As,ATAs,&opt);

			//g=ATAs-ATy;
			dcopy(n*k,ATAs,1,g,1);
			daxpy(n*k,-1.0,ATy,1,g,1);

			//xp=x;    Axp=Ax;
			dcopy(n*k,x,1,xp,1);
			dcopy(m*k,Ax,1,Axp,1);

			for(;;){
				//v=s-g/L;
				dcopy(n*k,s,1,v,1);
				daxpy(n*k,-1.0/L,g,1,v,1);

				//eppMatrix(v, n, k, lambda/ L, q);
				//eppMatrix(x,v,k,n,lambda/L,q);
				eppMatrix(x,v,n,k,lambda/L,q);


				//v=x-s;
				dcopy(n*k,x,1,v,1);
				daxpy(n*k,-1.0,s,1,v,1);

				// compute Ax
				normalizedMM('N',m,n,k,A,x,Ax,&opt);

				//Av=Ax -As;
				dcopy(m*k,Ax,1,Av,1);
				daxpy(m*k,-1.0,As,1,Av,1);

				//r_sum=norm(v,'fro')^2; l_sum=norm(Av,'fro')^2;
				r_sum = ddot(n*k,v,1,v,1);
				l_sum = ddot(m*k,Av,1,Av,1);
				if (r_sum <= 1.0e-20){
					bFlag=1; //% this shows that, the gradient step makes little improvement
					break;
				}
				if(l_sum <= r_sum * L)
					break;
				else
					L=maxof2(2.0*L, l_sum/r_sum);
			}
			ValueL[iterStep]=L;

			alphap=alpha; 
			alpha = (1.0+ sqrt(4.0*alpha*alpha +1.0))/2.0;

			//xxp=x-xp;   Axy=Ax-y;
			dcopy(n*k,x,1,xxp,1);
			daxpy(n*k,-1.0,xp,1,xxp,1);
			dcopy(m*k,Ax,1,Axy,1);
			daxpy(m*k,-1.0,y,1,Axy,1);

			funVal[iterStep] = ddot(m*k,Axy,1,Axy,1)/2.0;
			for(i=0; i<n; i++ )
				funVal[iterStep] += lambda* dnrm(k,&x[i],n,q);
			
			fValp = fVal = funVal[iterStep] ;
			if(iterStep != 0)
				fValp = funVal[iterStep -1];
			if(terminationCondition(&opt,fVal,fValp,x,dnrm2(n*k,xxp,1),dnrm2(n*k,xp,1),iterStep))
				break;
		}
	}
	ret.errorCode = 0;
	ret.totIter = iterStep;
	strcpy(ret.type,"mcLeastR");
	ret.ValueL = ValueL;
	ret.x = x;
	ret.funVal = funVal;
	return ret;
}





FUNVAL mcLeastRm2(double* A, double* y,double z,OPTS opt){
	int m, n, iterStep, k, i, bFlag;
	double q, q_bar, lambda, lambda_max, alpha, alphap, beta, L;
	double *ATy, *x, *Ax, *xp, *Axp, *xxp, *t, *tp, *s_t, *g, *s, *u, *v, *Av, *As, *ATAs, *Axy;
	double *ValueL, *funVal;
	double fVal, fValp, r_sum, l_sum;
	FUNVAL ret;

	m = opt.m;
	n = opt.n;
	k = opt.mc_k;

	ATy = (double *) malloc( sizeof(double) * n*k);
	x = (double *) malloc( sizeof(double) * n*k);
	Ax = (double *) malloc( sizeof(double) * m*k);
	xp = (double *) malloc( sizeof(double) * n*k);
	Axp = (double *) malloc( sizeof(double) * m*k);
	xxp = (double *) malloc( sizeof(double) * n*k);
	t = (double *) malloc( sizeof(double) * n);
	tp = (double *) malloc( sizeof(double) * n);
	s = (double *) malloc( sizeof(double) * n*k);
	s_t = (double *) malloc( sizeof(double) * n);
	As = (double *) malloc( sizeof(double) * m*k);
	ATAs = (double *) malloc( sizeof(double) * n*k);
	g = (double *) malloc( sizeof(double) * n*k);
	u = (double *) malloc( sizeof(double) * n*k);
	v = (double *) malloc( sizeof(double) * n*k);
	Av = (double *) malloc( sizeof(double) * m*k);
	ValueL = (double *) malloc( sizeof(double) * opt.maxIter);
	Axy = (double *) malloc( sizeof(double) * m*k);
	funVal = (double *) malloc( sizeof(double) * opt.maxIter);


	if(z<= 0)
		errorOut("z should be positive.");

	if(!hasGot(&opt,"q"))
		opt.q = q = 2;
	else if( (q = opt.q) < 1)
		errorOut("'q' should be larger than 1.");

	initNormalization(&opt,A);

	// calculate ATy
	normalizedMM('T',m,n,k,A,y,ATy,&opt);

	// Process regularization parameters
	if( opt.rFlag == 0 )
		lambda = z;
	else{
		if(z<0.0 || z>1.0 )
			errorOut("Z should lie in range [0,1] in mcLeastR.");
		if(q==1)
			q_bar= DBL_MAX;
		else if (q>1.0e6)
			q_bar = 1.0;
		else
			q_bar = q/(q-1);
		lambda_max = 0.0;
		for(i=0; i < n ; i++)
			lambda_max = maxof2( lambda_max , dnrm(k,&ATy[i],n,q_bar));
		lambda = z* lambda_max;
	}

	if( opt.init == 2)
		memset(x,0,n*k*sizeof(double));
	else if(hasGot(&opt,"x0"))
		dcopy(n*k,opt.x0,1,x,1);
	else
		dcopy(n*k,ATy,1,x,1);

	// compute Ax
	normalizedMM('N',m,n,k,A,x,Ax,&opt);

	if(opt.init == 0){
		// TO-DO have to do the initfactor module here
	}

	bFlag = 0;

	if (opt.mFlag==1 && opt.lFlag==0 && opt.q==2){

		L=1;

		//xp=x; Axp=Ax; xxp=zeros(n,k);
		dcopy(n*k,x,1,xp,1);
		dcopy(m*k,Ax,1,Axp,1);
		memset(xxp,0,n*k*sizeof(double));

		alphap=0; alpha=1;

		//t=sqrt(sum(x.^2, 2)); tp=t;
		for(i=0; i<n ; i++)
			t[i] = dnrm2(k,&x[i],n);
		dcopy(n,t,1,tp,1);

		for(iterStep = 0 ; iterStep < opt.maxIter ; iterStep ++ ){
			beta=(alphap-1)/alpha;

			//s=x + beta* xxp; s_t= t + beta * (t-tp);
			dcopy(n*k,x,1,s,1);
			daxpy(n*k,beta,xxp,1,s,1);
			dcopy(n,t,1,s_t,1);
			dscal(n,1.0 + beta, s_t,1);
			daxpy(n,-beta,tp,1,s_t,1);

			//As=Ax + beta* (Ax-Axp);
			dcopy(m*k,Ax,1,As,1);
			dscal(m*k,1.0 + beta, As,1);
			daxpy(m*k,-beta,Axp,1,As,1);

			// Compute ATAs
			normalizedMM('T',m,n,k,A,As,ATAs,&opt);

			//g=ATAs-ATy;
			dcopy(n*k,ATAs,1,g,1);
			daxpy(n*k,-1.0,ATy,1,g,1);

			//xp=x;    Axp=Ax; tp=t;
			dcopy(n*k,x,1,xp,1);
			dcopy(m*k,Ax,1,Axp,1);
			dcopy(n,t,1,tp,1);

			for(;;){
				//u=s-g/L;     v= s_t - lambda / L;
				dcopy(n*k,s,1,u,1);
				daxpy(n*k,-1.0/L,g,1,u,1);
				dcopy(n,s_t,1,v,1);
				daxpy(n,-lambda/L,oneVector,1,v,1);

				ep21R(x,t,u,v,n,k);

				//v=x-s;
				dcopy(n*k,x,1,v,1);
				daxpy(n*k,-1.0,s,1,v,1);

				// Compute Ax
				normalizedMM('N',m,n,k,A,x,Ax,&opt);

				//Av=Ax -As;
				dcopy(m*k,Ax,1,Av,1);
				daxpy(m*k,-1.0,As,1,Av,1);

				//r_sum=norm(v,'fro')^2 + norm(t-s_t)^2; l_sum=norm(Av,'fro')^2;
				r_sum = ddot(n*k,v,1,v,1) - 2.0*ddot(n,t,1,s_t,1) + ddot(n,t,1,t,1) + ddot(n,s_t,1,s_t,1);
				l_sum = ddot(m*k,Av,1,Av,1);
				if (r_sum <=1.0e-20){
					bFlag=1; //% this shows that, the gradient step makes little improvement
					break;
				}

				if(l_sum <= r_sum * L)
					break;
				else
					L=maxof2(2.0*L, l_sum/r_sum);
			}

			ValueL[iterStep] = L;

			alphap=alpha; alpha= (1.0 + sqrt(4.0*alpha*alpha +1.0))/2.0;

			//xxp=x-xp;   Axy=Ax-y;
			dcopy(n*k,x,1,xxp,1);
			daxpy(n*k,-1.0,xp,1,xxp,1);
			dcopy(m*k,Ax,1,Axy,1);
			daxpy(m*k,-1.0,y,1,Axy,1);

			//funVal(iterStep)=norm(Axy,'fro')^2/2 +  lambda * sum(t);
			fValp = fVal = funVal[iterStep]= lambda* ddot(n,t,1,oneVector,1) + ddot(m*k,Axy,1,Axy,1)/2.0;

			if(iterStep != 0 )
				fValp = funVal[iterStep - 1];

			if(terminationCondition(&opt,fVal,fValp,x,dnrm2(n*k,xxp,1),dnrm2(n*k,xp,1),iterStep))
				break;
		}
	}
	ret.errorCode = 0;
	ret.totIter = iterStep;
	strcpy(ret.type,"mcLeastR");
	ret.ValueL = ValueL;
	ret.x = x;
	ret.funVal = funVal;
	return ret;
}






FUNVAL mcLeastRm3(double* A, double* y,double z,OPTS opt){

	int m,n,k,iterStep,i, bFlag;
	double q,q_bar,lambda,lambda_max, L, gamma, alpha, alphap, beta, fVal, fValp, tao, r_sum, l_sum;
	double *ATy, *x, *Ax, *xp, *Axp, *xxp, *t, *tp, *ATAx, *ATAxp,
		*s, *s_t, *As, *ATAs, *g, *u, *v, *xnew, *tnew, *Axnew, *Av, *v_t, *Axy, *funVal, *ValueL;
	FUNVAL ret;

	m = opt.m;
	n = opt.n;
	k = opt.mc_k;

	ATy = (double * ) malloc( sizeof(double) * n*k);
	x = (double * ) malloc( sizeof(double) * n*k);
	Ax = (double * ) malloc( sizeof(double) * m*k);
	xp = (double * ) malloc( sizeof(double) * n*k);
	Axp = (double * ) malloc( sizeof(double) * m*k);
	xxp = (double * ) malloc( sizeof(double) * n*k);
	t = (double * ) malloc( sizeof(double) * n);
	tp = (double * ) malloc( sizeof(double) * n);
	ATAx = (double * ) malloc( sizeof(double) * n*k);
	ATAxp = (double * ) malloc( sizeof(double) * n*k);
	s = (double * ) malloc( sizeof(double) * n*k);
	s_t = (double * ) malloc( sizeof(double) * n);
	As = (double * ) malloc( sizeof(double) * m*k);
	ATAs = (double * ) malloc( sizeof(double) * n*k);
	g = (double * ) malloc( sizeof(double) * n*k);
	u = (double * ) malloc( sizeof(double) * n*k);
	v = (double * ) malloc( sizeof(double) * m*k);
	tnew = (double * ) malloc( sizeof(double) * n);
	xnew = (double * ) malloc( sizeof(double) * n*k);
	v_t = (double * ) malloc( sizeof(double) * n);
	Axnew = (double * ) malloc( sizeof(double) * m*k);
	Av = (double * ) malloc( sizeof(double) * m*k);
	ValueL = (double * ) malloc( sizeof(double) * opt.maxIter);
	Axy = (double * ) malloc( sizeof(double) * m*k);
	funVal = (double * ) malloc( sizeof(double) * opt.maxIter);


	if(z<= 0)
		errorOut("z should be positive.");

	if(!hasGot(&opt,"q"))
		opt.q = q = 2;
	else if( (q = opt.q) < 1)
		errorOut("'q' should be larger than 1.");

	initNormalization(&opt,A);

	// calculate ATy
	normalizedMM('T',m,n,k,A,y,ATy,&opt);

	// Process regularization parameters
	if( opt.rFlag == 0 )
		lambda = z;
	else{
		if(z<0.0 || z>1.0 )
			errorOut("Z should lie in range [0,1] in mcLeastR.");
		if(q==1)
			q_bar= DBL_MAX;
		else if (q>1.0e6)
			q_bar = 1.0;
		else
			q_bar = q/(q-1);
		lambda_max = 0.0;
		for(i=0; i < n ; i++)
			lambda_max = maxof2( lambda_max , dnrm(k,&ATy[i],n,q_bar));
		lambda = z* lambda_max;
	}

	if( opt.init == 2)
		memset(x,0,n*k*sizeof(double));
	else if(hasGot(&opt,"x0"))
		dcopy(n*k,opt.x0,1,x,1);
	else
		dcopy(n*k,ATy,1,x,1);

	// compute Ax
	normalizedMM('N',m,n,k,A,x,Ax,&opt);

	if(opt.init == 0){
		// TO-DO have to do the initfactor module here
	}

	bFlag = 0;

		
	if (opt.mFlag==1 && opt.lFlag==1 && opt.q==2){
		L= 1.0;
		gamma = 1.0;

		//xp=x; Axp=Ax;
		dcopy(n*k,x,1,xp,1);
		dcopy(m*k,Ax,1,Axp,1);
		memset(xxp,0,n*k*sizeof(double));

		//t=sqrt(sum(x.^2, 2)); tp=t;
		for(i=0; i<n ; i++)
			t[i] = dnrm2(k,&x[i],n);
		dcopy(n,t,1,tp,1);

		// Compute ATAx
		normalizedMM('T',m,n,k,A,Ax,ATAx,&opt);

		for(iterStep = 0 ; iterStep < opt.maxIter ; iterStep ++ ){
			//ATAxp = ATAx;
			dcopy(n*k,ATAx,1,ATAxp,1);

			// Compute ATAx
			normalizedMM('T',m,n,k,A,Ax,ATAx,&opt);

			// line search for L begins
			for(;;){
				if(iterStep !=  0){
					alpha= (-gamma+ sqrt(gamma*gamma + 4.0l * L * gamma)) / (2.0l*L);
					beta= (gamma - gamma* alphap) / (alphap * gamma + alphap* L * alpha);

					//s=x + beta * xxp;   s_t= t + beta * (t -tp);
					dcopy(n*k,x,1,s,1);
					daxpy(n*k,beta,xxp,1,s,1);

					dcopy(n,t,1,s_t,1);
					dscal(n,1.0 + beta, s_t,1);
					daxpy(n,-beta,tp,1,s_t,1);

					//As=Ax + beta* (Ax-Axp);
					dcopy(m*k,Ax,1,As,1);
					dscal(m*k,1.0 + beta,As,1);
					daxpy(m*k,-beta,Axp,1,As,1);

					//ATAs=ATAx + beta * (ATAx- ATAxp);
					dcopy(n*k,ATAx,1,ATAs,1);
					dscal(n*k,1.0 + beta,ATAs,1);
					daxpy(n*k,-beta,ATAxp,1,ATAs,1); 
					// changed here to minus beta and n to n*k This was the fucking problem.. :( not solved yet more problems.. dunno wat ------------------------------------------
				}
				else{
					alpha= (-1.0l + sqrt(5.0l)) / 2.0l;
					beta=0.0; 
					//s=x; s_t=t; As=Ax; ATAs=ATAx;
					dcopy(n*k,x,1,s,1);
					dcopy(n,t,1,s_t,1);
					dcopy(m*k,Ax,1,As,1);
					dcopy(n*k,ATAx,1,ATAs,1);
				}

				//g=ATAs-ATy;
				dcopy(n*k,ATAs,1,g,1);
				daxpy(n*k,-1.0l,ATy,1,g,1);

				//u=s-g / L;
				dcopy(n*k,s,1,u,1);
				daxpy(n*k,-1.0l/L,g,1,u,1);

				//v= s_t - lambda / L;
				dcopy(n,s_t,1,v,1);
				daxpy(n,-lambda/L,oneVector,1,v,1);

				ep21R(xnew,tnew,u,v,n,k);

				//v=xnew-s;
				dcopy(n*k,xnew,1,v,1);
				daxpy(n*k,-1.0,s,1,v,1);

				//v_t=tnew-s_t;
				dcopy(n,tnew,1,v_t,1);
				daxpy(n,-1.0,s_t,1,v_t,1);

				// Compute Axnew
				normalizedMM('N',m,n,k,A,xnew,Axnew,&opt);

				//Av=Axnew -As;
				dcopy(m*k,Axnew,1,Av,1);
				daxpy(m*k,-1.0,As,1,Av,1);

				//r_sum=norm(v,'fro')^2 + v_t'*v_t; l_sum=norm(Av,'fro')^2;
				r_sum = ddot(n*k,v,1,v,1) + ddot(n,v_t,1,v_t,1);
				l_sum = ddot(m*k,Av,1,Av,1);

				if (r_sum <=1.0e-20){
					bFlag=1; //% this shows that, the gradient step makes little improvement
					break;
				}
				if(l_sum <=  r_sum * L  )
					break;
				else
					L=maxof2(2.0 * L , l_sum/r_sum);
			}

			gamma=L* alpha* alpha;    
			alphap=alpha;

			ValueL[iterStep ] = L;


			tao=L * r_sum / l_sum;
			if (tao >=5.0)
				L=L*0.8;

			//xp=x;   x=xnew;  xxp=x-xp; 
			dcopy(n*k,x,1,xp,1);
			dcopy(n*k,xnew,1,x,1);
			dcopy(n*k,x,1,xxp,1);
			daxpy(n*k,-1.0,xp,1,xxp,1);
			//Axp=Ax; Ax=Axnew;
			dcopy(m*k,Ax,1,Axp,1);
			dcopy(m*k,Axnew,1,Ax,1);

			//tp=t; t=tnew;
			dcopy(n,t,1,tp,1);
			dcopy(n,tnew,1,t,1);

			//Axy=Ax-y;        
			dcopy(m*k,Ax,1,Axy,1);
			daxpy(m*k,-1.0,y,1,Axy,1);

			//funVal(iterStep)=norm(Axy,'fro')^2/2 + lambda * sum(t);
			fValp = fVal = funVal[iterStep] = lambda* ddot(n,t,1,oneVector,1) + ddot(m*k,Axy,1,Axy,1)/2.0;

			if(iterStep != 0)
				fValp = funVal[iterStep-1];

			if(terminationCondition(&opt,fVal,fValp,x,dnrm2(n*k,xxp,1),dnrm2(n*k,xp,1),iterStep))
				break;
		}
	}
	ret.errorCode = 0;
	ret.totIter = iterStep;
	strcpy(ret.type,"mcLeastR");
	ret.ValueL = ValueL;
	ret.x = x;
	ret.funVal = funVal;
	return ret;
}

